# -*- coding: utf-8 -*-
"""
Created on Mon Sep 25 22:22:43 2017

@author: hyj
"""

# -*- coding: utf-8 -*-
"""
Created on Sat Jul  1 14:02:03 2017

@author: hyj
"""

import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

#a = np.loadtxt('/home/hyj/opencv project/vo2/bin/odom.txt')
#fig = plt.figure()
#ax = fig.add_subplot(111)  
#ax.plot(a)

x=[]
y=[]
z=[]                                                                                                

#/home/hyj/slam_project/MarkerSLAM/bin/markerba.txt
#/home/hyj/slam_project/markervo/bin/campose_marker
with open('/media/hyj/DISK_IMG/vo/encoder.txt', 'r') as f:   #/home/hyj/opencv project/vo2/bin/odom_fuse_restart.txt  /home/hyj/slam_backend/isam2_sensorfusion/bin/isam2_fuse.txt
    data = f.readlines()  #txt中所有字符串读入data  
  
    for line in data:  
        odom = line.split(',')        #将单个数据分隔开存好  
        numbers_float = map(float, odom) #转化为浮点数  
        x.append( numbers_float[1+4] )
        y.append( numbers_float[2+4] )
        z.append( numbers_float[3+4] )

x1 = []
y1= []
z1=[]
#/home/hyj/opencv project/vo2/bin/vo_graph.txt
#poses_optimized_ceres_swf
with open('/home/hyj/slam_project/vo_and_odocal/svo_edge/bin/campose_readface_inodo.txt', 'r') as f:  
    data = f.readlines()  #txt中所有字符串读入data  
  
    for line in data:  
        odom = line.split()        #将单个数据分隔开存好  
        numbers_float = map(float, odom) #转化为浮点数  
        x1.append( numbers_float[1] )
        y1.append( numbers_float[2] )
        z1.append( numbers_float[3] )

x2 = []
y2= []
z2=[]
#/home/hyj/opencv project/vo2/bin/vo_graph.txt
#poses_optimized_ceres_swf
with open('/home/hyj/slam_project/vo_and_odocal/svo_edge/bin/campose_readface.txt', 'r') as f:  
    data = f.readlines()  #txt中所有字符串读入data  
  
    for line in data:  
        odom = line.split()        #将单个数据分隔开存好  
        numbers_float = map(float, odom) #转化为浮点数  
        x2.append( numbers_float[1] )
        y2.append( numbers_float[2] )
        z2.append( numbers_float[3] )
        
fig2d = plt.figure()
ax2d = fig2d.gca()
#ax2d.set_title("odom_fuse_restart")    
ax2d.set_xlabel('x [m]')
ax2d.set_ylabel('y [m]')
#ax2d.plot(x,y, c='r', label='opt')

ax2d.plot(x,y, c='g', label='odom')
ax2d.plot(x1,y1, c='r', label='vo_inodo') # vo
ax2d.plot(x2,y2, c='b', label='vo')
#ax2d.plot(laser_x,laser_y, c='b', label='laser slam')

#plt.xticks(np.arange(-0.5, 3.5, 0.5))
#plt.yticks(np.arange(-0.5, 1.5, 0.5))

leg = ax2d.legend(loc="upper left")
plt.show()
        
## plot 3d        
#fig = plt.figure()
#ax = fig.gca(projection='3d')
#ax.set_xlabel('x [m]')
#ax.set_ylabel('y [m]')
#ax.set_zlabel('z [m]')
#
#ax.plot(x1, y1, z1,c='black', label='odom')
#ax.plot(x, y, z,c='g', label='svo')
#ax.plot(x2, y2, z2,c='r', label='our method')
#ax.plot(laser_x, laser_y, laser_z,c='b', label='laser slam')
#ax.legend(loc="upper left")
#plt.show() 


